/*
 * miniFreeRTOS
 * Copyright (c) 2024- yanl1229@163.com.
 * Written by yanl1229
 *
 * This code is licensed under the GPL.
 */

#include <stdlib.h>
#include <string.h>

#include "task.h"
#include "queue.h"
#include "mm.h"


static inline void  queue_lock(struct msg_queue *queue)
{
    disable_interrupt();
    {
        if (queue->read_lock == queueUNLOCKED)
        {
            queue->read_lock = queueLOCKED_UNMODIFIED;
        }

        if (queue->write_lock == queueUNLOCKED)
        {
            queue->write_lock = queueLOCKED_UNMODIFIED;
        }
    }
    enable_interrupt();
}

static void queue_unlock(struct msg_queue *queue)
{
    disable_interrupt();
    {
        int8_t write_lock = queue->write_lock;

        /* See if data was added to the queue while it was locked. */
        while(write_lock > queueLOCKED_UNMODIFIED)
        {
            {
                /*
                 * 等待接收消息的任务队列不为空
                 */
                if (list_is_empty(queue->q_receivers) == 0)
                {
                    /* 从阻塞队列删除，重新加入就绪队列 */
                    if (remove_from_event_queue(queue->q_receivers))
                    {
                        yield();
                    }
                }
                else
                {
                    break;
                }
            }

            --write_lock;
        }

        queue->write_lock = queueUNLOCKED;
    }
    enable_interrupt();

    /* Do the same for the Rx lock. */
    disable_interrupt();
    {
        int8_t read_lock = queue->read_lock;

        while(read_lock > queueLOCKED_UNMODIFIED)
        {
            /* 等待发送消息的任务队列不为空 */
            if (list_is_empty(queue->q_senders) == 0)
            {
                if (remove_from_event_queue(queue->q_senders))
                {
                    yield();
                }
                --read_lock;
            }
            else
            {
                break;
            }
        }

        queue->read_lock = queueUNLOCKED;
    }
    enable_interrupt();
}

/**
 * 判断消息队列是否为空
 * 返回值:
 *    true:     队列空
 *    fasle:    队列非空
 */
static uint32_t queue_is_empty(const struct msg_queue * queue)
{
    uint32_t ret = 0;

    disable_interrupt();
    {
        /* 消息数为0， 消息队列为空 */
        if (queue->messages == 0)
        {
            ret = 1;
        }
        else
        {
            ret = 0;
        }
    }
    enable_interrupt();

    return ret;
}

/* 队列是否为满 */
static uint32_t queue_is_full(const struct msg_queue *queue)
{
    uint32_t ret;

    disable_interrupt();
    {
        if (queue->messages == queue->limit)
        {
            ret = 1;
        }
        else
        {
            ret = 0;
        }
    }
    enable_interrupt();

    return ret;
}


static inline void prvIncrementQueueTxLock(struct msg_queue * queue, uint32_t write_lock)
{
    const uint32_t uxNumberOfTasks = get_tasks();

    if (write_lock < uxNumberOfTasks)
        queue->write_lock = write_lock + 1;
}


static inline void prvIncrementQueueRxLock(struct msg_queue * queue, uint32_t read_lock)
{
    const uint32_t uxNumberOfTasks = get_tasks();

    if (read_lock < uxNumberOfTasks)
        queue->read_lock = read_lock + 1;

}

static void queue_reset(struct msg_queue * queue)
{

    if ((queue != NULL) && (queue->limit >= 1U) &&
        ((SIZE_MAX / queue->limit) >= queue->item_size))
    {
        sched_suspend();
        {
            queue->tail = queue->front + (queue->limit * queue->item_size);
            queue->messages = 0;
            /* 复位队列的存放消息的写位置和读位置 */
            queue->write_pos = queue->front;
            queue->read_pos = queue->front + ((queue->limit - 1U ) * queue->item_size);
            queue->read_lock = queueUNLOCKED;
            queue->write_lock = queueUNLOCKED;
            list_init(&queue->q_senders);
            list_init(&queue->q_receivers);
        }
        sched_resume();
    }
}

/**
 * @limit: 队列中元素个数
 * @item_size: 队列元素的长度
 * @buff: 队列存放消息的起始位置
 * @queue: 队列管理结构
 */
static void queue_init(uint32_t limit, uint32_t item_size,
                            uint8_t * buff, struct msg_queue * queue)
{
    /**
     * 设置队列中存放消息的位置指针
     */
    queue->front = (int8_t *)buff;
    /**
     * 设置队列个数容量和元素长度
     */
    queue->limit = limit;
    queue->item_size = item_size;
    queue_reset(queue);
}

/**
 * queue_create: 创建队列
 * @limit:  队列元素个数
 * @item_size:     队列的元素的长度
*/
struct msg_queue * queue_create(uint32_t limit, uint32_t item_size)
{
    struct msg_queue *queue = NULL;
    size_t buff_size;
    uint8_t *buff;

    if ((limit > 0) &&
        ((SIZE_MAX / limit) >= item_size) &&
        ((SIZE_MAX - sizeof(struct msg_queue)) >= (limit * item_size)))
    {
        /**
         * 计算整个存放消息的长度
         */
        buff_size = (size_t)(limit * item_size);
        queue = (struct msg_queue *)kmalloc(sizeof(struct msg_queue) + buff_size);
        if (queue != NULL)
        {
            /**
             * 队列存放消息的内存紧挨在消息队列管理结构的后面
             */
            buff = (uint8_t *)queue + sizeof(struct msg_queue);
            queue_init(limit, item_size, buff, queue);
        }
    }

    return queue;
}

/**
 * 拷贝消息到队列中
 */
static void enqueue_message(struct msg_queue *queue, void *msg)
{
    uint32_t messages;

    /* This function is called from a critical section. */

    messages = queue->messages;


    /* 拷贝到队列存储消息区域的尾部 */
    memcpy((void *)queue->write_pos, msg, (size_t)queue->item_size);
    queue->write_pos += queue->item_size;

    /* 检查队列是否为满, 如果满了,从头开始覆盖,类似于循环fifo */
    if (queue->write_pos >= queue->tail)
    {
        queue->write_pos = queue->front;
    }

    queue->messages = messages + 1;
}

/**
 * 队列中的消息拷贝到msg中
 */
static void dequeue_message(struct msg_queue *queue, void *msg)
{
    if (queue->item_size != 0)
    {
        queue->read_pos += queue->item_size;

        /* 队列为空，复位读位置指针 */
        if (queue->read_pos >= queue->tail)
        {
            queue->read_pos = queue->front;
        }

        memcpy((void *)msg, (void *)queue->read_pos, (size_t)queue->item_size);
    }
}

/**
 * @msg: 消息
 * @wait_ticks: 等待发送超时tick
*/
static uint32_t queue_common_write(struct msg_queue * queue, void *msg, int noblock)
{

    for( ; ; )
    {
        disable_interrupt();
        {
            /* 判断队列是否有足够的空间，存放发送的消息 */
            if ((queue->messages < queue->limit))
            {

                enqueue_message(queue, msg);
                /*
                 * 有任务阻塞在等待消息,尝试唤醒等待消息的任务
                 */
                if (!list_is_empty(queue->q_receivers))
                {
                    /* 有高优先级任务存在，直接抢占当前任务 */
                    if (remove_from_event_queue(queue->q_receivers))
                        yield();
                }

                enable_interrupt();
                /* 发送消息成功 */
                return 1;
            } else {
                if (noblock == 1) {
                    enable_interrupt();
                    return 0;
                }
            }
        }
        enable_interrupt();

        /*
         * 发送失败，此时队列已满，发送任务尝试延迟发送
         */
        sched_suspend();
        /* 队列上锁 */
        queue_lock(queue);

        /*队列满 */
        if (queue_is_full(queue))
        {
            /* 加入到等待发送的任务队列 */
            list_add_tail(queue->q_senders, &current->event_node);
            /*
             * 队列解锁
             */
            queue_unlock(queue);
            /* 当前发送任务已经加入等待发送队列，调度器恢复后，直接触发任务调度 */
            if (sched_resume() == 0)
                yield();
        }
        else
        {
            /* 队列未满, 继续尝试发送 */
            queue_unlock(queue);
            sched_resume();
        }
    }
}

uint32_t queue_write_noblock(struct msg_queue * queue, void *msg)
{
    return queue_common_write(queue, msg, 1);
}
/**
 * @msg: 消息
 * @wait_ticks: 等待发送超时tick
*/
uint32_t queue_write(struct msg_queue * queue, void *msg)
{
    return queue_common_write(queue, msg, 0);
}

/**
 * 从队列中读消息
 * @buff: 存放读到的消息缓存
*/
static uint32_t queue_common_read(struct msg_queue * queue, void *buff, int noblock)
{

    for( ; ; )
    {
        disable_interrupt();
        {
            uint32_t messages = queue->messages;

            /**
             * 队列中存在消息
             */
            if (messages > 0)
            {
                dequeue_message(queue, buff);
                /* 队列现存消息数减一 */
                queue->messages = messages - 1;

                /**
                 * 入队等待队列不为空，说明之前的队列是满的，本次的读操作，会腾出来一个位置，将
                 * 可以唤醒一个等待发送消息的任务
                 */
                if (list_is_empty(queue->q_senders) == 0)
                {
                    if (remove_from_event_queue(queue->q_senders))
                    {
                        yield();
                    }
                }

                enable_interrupt();
                return 1;
            } else {
                if (noblock == 1) {
                    enable_interrupt();
                    return 0;
                }
            }
        }
        enable_interrupt();

        sched_suspend();
        queue_lock(queue);
        /*
         * 未超时，但是消息队列为空， 这时候，只能加入到等待接收阻塞队列中
         */
        if (queue_is_empty(queue))
        {
            /* 当前任务加入到阻塞等待队列,并让出CPU */
            dequeue_task(current);
            list_add_tail(queue->q_receivers, &current->event_node);
            queue_unlock(queue);
            if (sched_resume() == 0)
                yield();
        }
        else
        {
            /* 队列不为空，再次尝试读取 */
            queue_unlock(queue);
            sched_resume();
        }
    }
}

uint32_t queue_read_noblock(struct msg_queue * queue, void *buff)
{
    return queue_common_read(queue, buff, 1);
}
/**
 * 从队列中读消息
 * @buff: 存放读到的消息缓存
*/
uint32_t queue_read(struct msg_queue * queue, void *buff)
{
    return queue_common_read(queue, buff, 0);
}

uint32_t queue_write_irq(struct msg_queue * queue, void *msg)
{
    uint32_t ret;
    uint32_t flags;


    flags = irq_save();
    {
        if ((queue->messages < queue->limit))
        {
            const int8_t write_lock = queue->write_lock;

            enqueue_message(queue, msg);

            if (write_lock == queueUNLOCKED)
            {
                if (!list_is_empty(queue->q_receivers))
                {
                    if (remove_from_event_queue(queue->q_receivers)) {
                        yield();
                    }
                }
                ret = 1;
            }
            else
            {
                prvIncrementQueueTxLock(queue, write_lock);
                ret = 1;
            }


        }
        else
        {
            ret = 0;
        }
    }
    irq_restore(flags);

    return ret;
}


uint32_t queue_read_irq(struct msg_queue * queue, void * msg)
{
    uint32_t ret;
    uint32_t flags;

    flags = irq_save();
    {
        uint32_t messages = queue->messages;

        /* Cannot block in an ISR, so check there is data available. */
        if (messages > 0)
        {
            int8_t read_lock = queue->read_lock;

            dequeue_message(queue, msg);
            queue->messages = messages - 1;

            if (read_lock == queueUNLOCKED)
            {
                if (list_is_empty(queue->q_senders) == 0)
                {
                    if (remove_from_event_queue(queue->q_senders)) {
                        yield();
                    }
                }
                ret = 1;
            }
            else
            {
                prvIncrementQueueRxLock(queue, read_lock);
                ret = 0;
            }


        }
        else
        {
            ret = 0;
        }
    }
    irq_restore(flags);

    return ret;
}

void queue_delete(struct msg_queue *queue)
{
    if (queue != NULL)
        kfree(queue);
}
